#ifdef USE_FREQER

#include <Arduino.h>
#include "freqer.h"
#include "noise_flt.h"

void TIMINPUT_Capture_Rising_IT_callback(HardwareTimer *MyTim)
{
    auto *tim = (freqer *)(MyTim);
    TIM_HandleTypeDef *tm = tim->getHandle();
    if (tm->Channel == HAL_TIM_ACTIVE_CHANNEL_1)
    {
        tim->timinput_capture_rising_it_callback(1);
    }

    if (tm->Channel == HAL_TIM_ACTIVE_CHANNEL_2)
    {
        tim->timinput_capture_rising_it_callback(2);
    }

    if (tm->Channel == HAL_TIM_ACTIVE_CHANNEL_3)
    {
        tim->timinput_capture_rising_it_callback(3);
    }

    if (tm->Channel == HAL_TIM_ACTIVE_CHANNEL_4)
    {
        tim->timinput_capture_rising_it_callback(4);
    }
}

void freqer::timinput_capture_rising_it_callback(int channelRising)
{
    CurrentCapture[channelRising] = count * 0x10000 + getCaptureCompare(channelRising);
    int v = CurrentCapture[channelRising] - LastPeriodCapture[channelRising];
    if (digitalRead(_pin[channelRising]))
    {
        buf[channelRising][binx] = v;
        LastPeriodCapture[channelRising] = CurrentCapture[channelRising];
        rolloverCompareCount[channelRising] = 0;
        binx++;
        binx %= 20;
    }
}

/* In case of timer rollover, frequency is to low to be measured set values to 0
   To reduce minimum frequency, it is possible to increase prescaler. But this is at a cost of precision. */
void Rollover_IT_callback(HardwareTimer *MyTim)
{
    auto *tim = (freqer *)(MyTim);
    tim->rollover_it_callback();
}

void freqer::rollover_it_callback()
{
    count += 1;
    for (int i = 0; i < 5; i++)
    {
        rolloverCompareCount[i]++;

        if (rolloverCompareCount[i] > 2)
        {
            FrequencyMeasured[i] = 0;
            DutycycleMeasured[i] = 0;
            buf[i][binx] = 0;
            binx++;
            binx %= 20;
        }
    }
}

freqer::freqer(TIM_TypeDef *instance) : HardwareTimer(instance)
{
    count = 0;
    for (unsigned char &i : _pin)
    {
        i = 0;
    }
}

int freqer::begin(int pin, uint32_t PrescalerFactor)
{
    int channelRising = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pin), PinMap_PWM));
    _pin[channelRising] = pin;
    // buf[channelRising][0].begin((uint32_t)10);
    // buf[channelRising][1].begin((uint32_t)10);
    fil[channelRising] = new SimpleKalmanFilter(1000, 100, 0.1);
    setMode(channelRising, TIMER_INPUT_CAPTURE_RISING, pin);
    setPrescaleFactor(PrescalerFactor);
    setOverflow(
        0x10000); // Max Period value to have the largest possible time to detect rising edge and avoid timer rollover
    attachInterrupt(channelRising, TIMINPUT_Capture_Rising_IT_callback);
    attachInterrupt(Rollover_IT_callback);
    resume();
    input_freq = getTimerClkFreq() / getPrescaleFactor();
    return 0;
}

float freqer::getFreq(int pin)
{
    int channelRising = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pin), PinMap_PWM));
    if (rolloverCompareCount[channelRising] < 2)
    {
        noise_flt<int> flt{};
        flt.begin(20, 0);
        float val = 0;
        for (int i = 0; i < 20; i++)
            flt.write(buf[channelRising][i]);
        float v = flt.median();
        FrequencyMeasured[channelRising] = (uint32_t)((float)input_freq / v);
        //        DutycycleMeasured[channelRising] = h / (h + l);
        return FrequencyMeasured[channelRising];
    }
    return 0;
}

#endif
